#pragma once
#include "ros/ros.h"

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

#include "yaml-cpp/yaml.h"
#include "control/control_params.h"
#include "control/custom_class.h"
#include "common_lib/common.h"
#include "common_lib/log.h"
#include "common_lib/transform.h"
#include "control_msgs/control_msg.h"
#include "control/move_control/path_following_base.h"

#include <tf/tf.h>

namespace control
{
    class LQRPathFollowing: public PathFollowingBase
    {
    public:
        LQRPathFollowing(ros::NodeHandle &nh, ros::NodeHandle &private_nh)
        : stop_distance_threshold_(0.01,0.03)
        {
			Init(nh);
        }
        ~LQRPathFollowing(){}
        
		void Init(ros::NodeHandle n) override;
		void VehicleController(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, float &speed, float &angle_speed, float &speed_toward, float &max_speed_limit, float &max_angle_speed_limit) override;
		void ReInit(){running_type_ = RunningType::HEADFOLLOWING;};
		void VehicleStop()
		{
			speed_ = 0;
			angle_speed_ = 0;
			// lqr_speed_smooth_.Reset(g_speed_plan_type);
		};
    private:
        common_lib::Common com_;
		common_lib::Transform tra_;
        common_lib::DoubleThreshold stop_distance_threshold_;

        common_lib::TON ton_switch_type_;
        common_lib::TON ton_zero_speed_;

		std::shared_ptr<common_lib::SpeedSmoothBase> ptr_speed_smooth_;
		std::shared_ptr<common_lib::SpeedSmoothBase> ptr_anglespeed_smooth_;

        bool is_in_dec_stage_ = false; // 是否处于减速阶段
		// trajectory_msgs::point_msg last_distance_dec_point_;    // 移动目标的最后一个点，减速阶段是保持不变的
		int last_traj_size_ = 0;                               // 判断路径尺寸，增加一些鲁棒性
		ros::Time last_loop_time_ = ros::Time::now();
		
        // 用于计算减速的参数
		float distance_dec_ = 0.0;     // 实际的减速距离
		float last_distance_dec_ = 4.0;  // 上一个周期的实际减速距离
		float predict_distance_dec = 0.0; // 上一个周期的预测本周期的减速距离
		float dec_start_speed_ = 0.0;     // 进入减速阶段，计算下个周期的速度的起点速度

        Eigen::Matrix4f LQR_Q_unload_;
		Eigen::Matrix4f LQR_Q_load_;
		Eigen::Matrix4f LQR_Curve_;

		void HeadFollowingController(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, float &speed, float &angle_speed, float &max_speed_limit, float &max_angle_speed_limit);
		void RotatingController(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, float &speed, float &angle_speed, float &max_angle_speed_limit);		
		bool IsFollowingToRotating(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state);
		bool IsRotatingToFollowing(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state);
		float TargetLongDistance(AMRState &amr_state,amr_trajectory_msgs::point_info target_point, float line_theta);

		void LineSpeedLimit(AMRState &amr_state, float &speed, float max_speed_limit) override;
		void StableBoost(AMRState &amr_state, float &speed);
		void JudgeStableBoost(AMRState &amr_state);

		void SetLQRQ();
		Eigen::Matrix4f DLQR(Eigen::Matrix4f A, Eigen::Vector4f B, Eigen::Matrix4f Q, float R);
    };
}